using System;
using Microsoft.SPOT;
using System.Threading;
using GHIElectronics.NETMF.Hardware;
using GPS_Module;
using BigBlueSparx.NetMF.FEZ.HMC6352Compass;

namespace FEZ_Cobra_Console_Application1
{
    /// <summary>
    /// Please adjust the values here when getting readouts
    /// </summary>
    public class Readouts
    {
        public bool sampling = false;
        public static bool initialized = false;
        static AnalogIn rangefinder = new AnalogIn((AnalogIn.Pin.Ain6));
        static AnalogIn ultrasonic = new AnalogIn((AnalogIn.Pin.Ain5));
        public static string GPS_Position;
        public static string Comp_Orientation;
        public static double Front_Rangefinder;
        public static double Rear_Rangefinder;
        
        public static string readings = "";
        GPS myGPS = new GPS();
        //HMC6352Compass compass = new HMC6352Compass();

        Thread SensorPolling;
        Thread GpsPolling;

        public static Readouts theInstance;   //for singleton class

        /// <summary>
        /// Re initializes all readout values. Private Singleton constructor, only ran once
        /// </summary>
        private Readouts()
        {    
            ultrasonic.SetLinearScale(0, 1023);
            rangefinder.SetLinearScale(0, 1023);
            GPS_Position = "00.000000,N,00.000000,E";
            Comp_Orientation = "00.000";
            Front_Rangefinder = 0;
            Rear_Rangefinder = 0;


            //singleton stuff
            initialized = true;
            theInstance = this;
        }

        public static Readouts getInstance()
        {
            if(!initialized)
                return new Readouts();
            else
                return theInstance;
        }

        public void startSampling()
        {
            //dont get more than one set of threads sampling
            if(sampling == true)
                return;

            sampling = true;

            //stat up the sampling threads
            SensorPolling = new Thread(new ThreadStart(CheckValues));
            SensorPolling.Start();

            GpsPolling = new Thread(new ThreadStart(refreshGPS));
            //GpsPolling.Start();

        }

        public void stopSampling()
        {
            //abort threads
            SensorPolling.Abort();
            GpsPolling.Abort();

            sampling = false;
            Thread.Sleep(1001);
        }

        public string getReadouts()
        {
            //order will be imporatant here
            readings = "BEACON " + getGPSformat() + " " + Comp_Orientation + " " + Front_Rangefinder.ToString("F4") + " " + Rear_Rangefinder.ToString();        
            return readings;
        }

        private void setGPS()
        {
            while (true)
            {
                myGPS.pollGPS(5);
            }

        }

        /// <summary>
        /// Returns formatted GPS data
        /// </summary>
        /// <returns></returns>
        public string getGPSformat()
        {
            return myGPS.Latitude.ToString() + "," + myGPS.NorSow.ToString() + "," + myGPS.Longitude.ToString() + "," + myGPS.EasWes.ToString();
        }

        //private void setComp()
        //{
        //    float heading = compass.GetHeading();
            
        //       // Debug.Print(heading.ToString());
        //    Comp_Orientation = heading.ToString();
            
            
        //    //Debug.Print(heading.ToString());


        //}

        private void setRear()
        {
            //Returns ultrasonic reading in ADC numbers
            Rear_Rangefinder = (ultrasonic.Read());
        }

        private void setFront()
        {
            //Returns Front Rangefinder reading value in inches
            Front_Rangefinder= (rangefinder.Read());
            if (Front_Rangefinder <= 770)             //Dropoff for rangefinder reading is about 8 inches, 770 on ADC
                Front_Rangefinder = ((69007) * (System.Math.Pow(Front_Rangefinder, (-1.357))));
            else Front_Rangefinder = 0.000;
        }

        public static double getIRReading()
        {
            return Front_Rangefinder;
        }

        /// <summary>
        /// Refreshes the values every period. Should be run in its own thread
        /// </summary>
        public void CheckValues()
        {
            while (sampling)
            {
                try
                {
                    setFront();
                    setRear();
                    //setComp();
                    Thread.Sleep(2000); //checking interval
                }
                catch
                {
                    Debug.Print("CheckValues just failed. Will try again next intreval");
                }
            }
        }
        
        /// <summary>
        /// Refreshes the GPS value. Should run in its own thread
        /// </summary>
        public void refreshGPS()
        {
            while (sampling)
            {
                setGPS();
                Thread.Sleep(1500); //Polling interval. This may change
            }
        }
    }
}
